﻿Imports System.Net.Sockets
Imports System.Threading
Imports System.Net
Imports System.IO

Public Class Form1
    Private Const TEAM_ROBOT_IP_ADDRESS As String = "10.11.3.2" 'Change this to match your robot's IP address
    Private Const DSPACKET_LENGTH As Integer = 1018
    Private Const DS_TO_PC_LOCAL_PORT As Integer = 1165
    Private Const DS_TO_PC_REMOTE_PORT As Integer = 1170

    Public Sub New()

        ' This call is required by the Windows Form Designer.
        InitializeComponent()

        txtIP.Text = TEAM_ROBOT_IP_ADDRESS
        ' Add any initialization after the InitializeComponent() call.

    End Sub
    Dim exitLoop As Boolean = False
    Dim DSThread As Thread
    Dim cameraThread As Thread

    Dim packetNumber As UInt16
    Dim teamNumber As UInt16
    Dim digitalIns As Byte
    Dim digitalOuts As Byte
    Dim battery As UInt16
    Dim statusByte As Byte
    Dim errorByte As Byte

    Dim versionMonth1 As Byte
    Dim versionMonth2 As Byte
    Dim versionDay1 As Byte
    Dim versionDay2 As Byte
    Dim versionYear1 As Byte
    Dim versionYear2 As Byte
    Dim versionRev1 As Byte
    Dim versionRev2 As Byte

    Dim unusedBuffer1 As UInt32
    Dim unusedBuffer2 As UInt16

    Dim replyPacketNumber As UInt16

    Dim userData As Byte()

    Dim cameraImage As Image

    Sub DoUpdate(ByVal result As IAsyncResult)

    End Sub

    Sub GetData()
        Dim client = New UdpClient(DS_TO_PC_LOCAL_PORT)
        Dim dsEndpoint = New IPEndPoint(IPAddress.Any, DS_TO_PC_REMOTE_PORT)
        While (True)
            Dim bytes = client.Receive(dsEndpoint)

            'confirm the length of the data packet is correct.
            If bytes.Length = DSPACKET_LENGTH Then
                Dim reader = New BinaryReader(New MemoryStream(bytes))

                packetNumber = reader.ReadUInt16()
                digitalIns = reader.ReadByte()
                digitalOuts = reader.ReadByte()
                battery = reader.ReadUInt16()

                statusByte = reader.ReadByte()
                errorByte = reader.ReadByte()

                'WARNING: do not inline temps. The order read from stream MUST be maintained.
                Dim temp1 = reader.ReadByte()
                Dim temp2 = reader.ReadByte()
                teamNumber = temp1 * 100 + temp2

                'TODO: Correct version parsing in test app
                '   I believe they are in the same format as the team number, but haven't confirmed.
                versionMonth1 = reader.ReadByte()
                versionMonth2 = reader.ReadByte()
                versionDay1 = reader.ReadByte()
                versionDay2 = reader.ReadByte()
                versionYear1 = reader.ReadByte()
                versionYear2 = reader.ReadByte()
                versionRev1 = reader.ReadByte()
                versionRev2 = reader.ReadByte()

                unusedBuffer1 = reader.ReadUInt32()
                unusedBuffer2 = reader.ReadUInt16()

                'I believe this is only important for robot-to-DS communication, but haven't confirmed.
                replyPacketNumber = reader.ReadUInt16()

                userData = reader.ReadBytes(984)

                'TODO: Figure out what the remaining 8 bytes are. Possibly checksum?
                reader.Close()
            End If
        End While
    End Sub

    Sub GetCamera()
        Dim client As New TcpClient()
        Dim imageCount As Integer
        client.Connect(txtIP.Text, 1180)

        Dim stream = client.GetStream()
        Dim reader = New BinaryReader(stream)

        'State machine state variables
        Dim headerPosition = 0
        Dim mode = 0

        Dim imageData As Byte()
        Dim imageDataLength As UInt32 = 0

        'Dim bytes = reader.ReadBytes(20)
        While (1)
            Select Case mode
                Case 0 'Header
                    imageData = Nothing
                    imageDataLength = 0

                    Dim b = reader.ReadByte()
                    'Debug.Print(b)
                    Select Case headerPosition
                        Case 0
                            If b = 1 Then
                                headerPosition += 1
                            Else
                                headerPosition = 0
                            End If
                        Case Is < 4
                            If b = 0 Then
                                headerPosition += 1
                            Else
                                headerPosition = 0
                            End If
                            If headerPosition = 4 Then
                                mode += 1
                                headerPosition = 0
                            End If
                        Case Else
                            Throw New InvalidOperationException()
                    End Select
                Case 1 'Read length
                    'imageDataLength = reader.ReadUInt32()
                    Dim buffer = reader.ReadBytes(4)
                    imageDataLength = buffer(0) * 255 ^ 3 + _
                                        buffer(1) * 255 ^ 2 + _
                                        buffer(2) * 255 + _
                                        buffer(3)
                    'Debug.Print(imageDataLength)
                    mode += 1
                Case 2 'Read image
                    imageCount += 1
                    Try
                        imageData = reader.ReadBytes(imageDataLength)
                        Dim memStream = New MemoryStream(imageData, 0, imageDataLength)

                        cameraImage = Image.FromStream(memStream)

                        memStream.Close()
                    Catch ex As Exception
                        Debug.Write("Image " & imageCount & ", length " & imageDataLength & ": " & ex.Message)
                    Finally
                        mode = 0
                    End Try
                    'cameraImage = Image.FromStream(reader.BaseStream)
            End Select


        End While

    End Sub

    Private Sub Button1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button1.Click
        DSThread = New Thread(AddressOf GetData)
        DSThread.Start()
    End Sub

    Private Sub Timer1_Tick(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Timer1.Tick
        txtTeamNum.Text = teamNumber
        txtBattery.Text = battery
        cameraPicture.Image = cameraImage

    End Sub

    Private Sub Button2_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button2.Click
        cameraThread = New Thread(AddressOf GetCamera)
        cameraThread.Start()
    End Sub

    Private Sub Form1_FormClosing(ByVal sender As Object, ByVal e As System.Windows.Forms.FormClosingEventArgs) Handles Me.FormClosing
        exitLoop = True
    End Sub
End Class